q = [0 0.5 0.4 0];
qp = [0 0 0 0];
qpp = qp;
g = 9.8;
m5 = 3;
Iext = 0.05*eye(3);
tau = newtoneuler6(q,qp,qpp,g,m5,Iext)